#!/usr/bin/python
import roslib; roslib.load_manifest('DelphinROSv2')
import rospy
import serial
import time
import numpy
import datetime
import math
import scipy
from scipy import stats
from DelphinROSv2.msg import tsl_setpoints
from std_msgs.msg import Float32
from std_msgs.msg import Bool
from DelphinROSv2.msg import compass as compass_out
from DelphinROSv2.msg import tail_setpoints

global new_compass
global new_mission
global cur_mission
global cur_vert
global cur_compass
global integral_error
global last_prop_error

global dt_array
global time_array
global heading_array
global controller_onOff


def low_level_control():
    global new_compass
    global new_mission
    global cur_headingd
    global cur_swayd
    global cur_compass
    global integral_error
    global last_prop_error
    global last_execute_time
    global controller_onOff
    
    global dt_array
    global time_array
    global heading_array
    
    # Initializing derivative arrays
    global der_array_size
    global time_array
    global sample
    der_array_size = 50
    time_array = numpy.zeros(der_array_size)
    sample = numpy.zeros(der_array_size)
   
        
    last_execute_time = datetime.datetime.now()
    last_prop_error = 0.0
    integral_error = 0.0
    cur_headingd = Float32()
    cur_swayd = Float32()
    cur_compass = compass_out()
    new_mission = False
    new_compass = False
    controller_onOff=Bool()

    pub = rospy.Publisher('TSL_setpoints_horiz', tsl_setpoints)
    pub_tail_setpoints = rospy.Publisher('tail_update', tail_setpoints)

    
    int_gain = rospy.get_param(rospy.resolve_name('horiz_integral-gain'))
    deriv_gain = rospy.get_param(rospy.resolve_name('horiz_deriv-gain'))
    prop_gain = rospy.get_param(rospy.resolve_name('horiz_prop-gain'))
    max_sat = rospy.get_param(rospy.resolve_name('horiz_max-saturation'))
    min_sat = rospy.get_param(rospy.resolve_name('horiz_min-saturation'))
    max_int_sat = rospy.get_param(rospy.resolve_name('horiz_max-int-saturation'))
    min_int_sat = rospy.get_param(rospy.resolve_name('horiz_min-int-saturation'))
    max_prop_sat = rospy.get_param(rospy.resolve_name('horiz_max-prop-saturation'))
    min_prop_sat = rospy.get_param(rospy.resolve_name('horiz_min-prop-saturation'))
    aft_dead_zone = rospy.get_param(rospy.resolve_name('horiz_aft-dead-zone'))
    fwd_dead_zone = rospy.get_param(rospy.resolve_name('horiz_fwd-dead-zone'))
    sway_min = rospy.get_param(rospy.resolve_name('horiz_sway-min-sat'))
    sway_max = rospy.get_param(rospy.resolve_name('horiz_sway-max-sat'))
    
    

    #print 'fwd_dead_zone %f' %(fwd_dead_zone)    
    
    while not rospy.is_shutdown():
                
        if new_compass or new_mission:
            new_compass = False
            output = tsl_setpoints()
            
            #print 'in lower loop, about to run pid controller...'
            pid_ret = pid_control(int_gain, deriv_gain, prop_gain, max_sat, min_sat, max_int_sat, min_int_sat, max_prop_sat, min_prop_sat, cur_headingd.data, cur_compass.heading, fwd_dead_zone)

            #combine sway 
            if cur_swayd.data<sway_min:
		cur_swayd.data=sway_min

	    if cur_swayd.data>sway_max:
		cur_swayd.data=sway_max
            output.thruster0 = int(pid_ret['setpoint']+cur_swayd.data)
            
            #RUDDER!!!
            rudderDemand = int(output.thruster0 / 50);
            if rudderDemand < -30:
                rudderDemand = -30
            elif rudderDemand > 30:
                rudderDemand = 30

            pid_ret = pid_control(int_gain, deriv_gain, prop_gain, max_sat, min_sat, max_int_sat, min_int_sat, max_prop_sat, min_prop_sat, cur_headingd.data, cur_compass.heading, aft_dead_zone)
            output.thruster1 = int(-pid_ret['setpoint']+cur_swayd.data)
            output.pterm = pid_ret['prop_term']
            output.iterm = pid_ret['integral_term']
            output.dterm = pid_ret['deriv_term']

            #print controller_onOff
            
            tailout=tail_setpoints()
            tailout.b=rudderDemand
            tailout.d=rudderDemand
            tailout.c=0
            tailout.e=0
            #print 'Rudder Demand', rudderDemand

            if controller_onOff==True:
            	pub.publish(output)
               # pub_tail_setpoints.publish(tailout)
                #print 'controller on'   


def get_time_interval(initial_time):
    return (datetime.datetime.now() - initial_time).microseconds / 1000000.0


def pid_control(int_gain, deriv_gain, prop_gain, max_sat, min_sat, max_int_sat, min_int_sat, max_prop_sat, min_prop_sat, demand, current_heading, dead_zone):
    prop_error = 0.0
    deriv_error = 0.0
    global integral_error
    global last_prop_error
    global last_execute_time

    dt = (datetime.datetime.now() - last_execute_time).microseconds / 1000000.0 # get seconds from the time interval
    last_execute_time = datetime.datetime.now()
    
    if dt != 0:

	# double check demand in range 0-360

	demand=demand%360

        #calulating heading error and check shortest direction
        prop_error = demand - current_heading

        if prop_error<-180:
		prop_error=prop_error%360
	if prop_error>180:
		prop_error=-(-prop_error%360)


	#print 'demand: ', demand
	#print 'heading: ', current_heading
        #print 'prop err %f' %(prop_error)


	#apply deadzone

	if math.fabs(prop_error)<dead_zone:
		prop_error=0


        #calulating proportional term
        prop_term = prop_gain * prop_error
        #print 'prop term before sat %f' %(prop_term)
        
        #limit to proportional saturation
        if prop_term < min_prop_sat:
            prop_term = min_prop_sat
        elif prop_term > max_prop_sat:
            prop_term = max_prop_sat
        #print 'prop term %f' %(prop_term)
        
        #calculating integral term
        #print 'int error before %f' %(integral_error)
        integral_error += dt * prop_error
        #print 'dt %f' %(dt)
        #print 'int error after %f' %(integral_error)
        integral_term = int_gain * integral_error
        #print 'int gain %f' %(int_gain)
    
    
        #print 'integral term before sat %f' %(integral_term)
        #limit to integral saturation
        if integral_term < min_int_sat:
            integral_term = min_int_sat
        elif integral_term > max_int_sat:
            integral_term = max_int_sat
        #print 'integral term %f' %(integral_term)
        
        
        #### DERIVATIVE ####
        time_array[1:der_array_size] = time_array[0:(der_array_size-1)]		# Shift old values up in the array
        time_array[0] = dt							#Set first array term to new dt value

        sample[1:der_array_size] = sample[0:(der_array_size-1)]			# Shift old values up in the array
	sample[0] = current_heading
        
	cumulative_time = numpy.zeros(der_array_size)				# Reset cumulative time array to zero
	
	i = 1
        #print 'entering extra while loop.....'
        while i < der_array_size:						# Create cumulative time array
            cumulative_time[i] = cumulative_time[i-1] + time_array[i]
            i += 1

	derivative, intercept, r_value, p_value, std_err = stats.linregress(cumulative_time,sample)	# Calculate linear least squares fit			
			
	dTerm = derivative * deriv_gain						# Calculate dTerm
        deriv_term = dTerm
        
        #print 'derivative', dTerm
        
        
        if not numpy.isnan(dTerm):
            #print 'dTerm exists: ', dTerm
            setpoint = prop_term + integral_term + deriv_term			# Calculate total linear controller output
        else:
           # print 'dTerm not exist'
            setpoint = prop_term + integral_term

       
        #limit total controller output to saturation
        if setpoint < min_sat:
            setpoint = min_sat
        elif setpoint > max_sat:
            setpoint = max_sat

                    
        return {'setpoint':setpoint, 'prop_term':prop_term, 'integral_term':integral_term, 'deriv_term':deriv_term}
    else:
        return {'setpoint':0, 'prop_term':0, 'integral_term':0, 'deriv_term':0} # unlikely.
        
def heading_demand_cb(headingd):
    global new_mission
    global cur_headingd
    cur_headingd = headingd
    new_mission = True

def sway_demand_cb(swayd):
    global new_mission
    global cur_swayd
    cur_swayd = swayd
    new_mission = True

def compass_cb(compass):
    global cur_compass
    global new_compass
    cur_compass = compass
    new_compass = True
    #print 'New compass data!!!!!!!!!!!!!!!!!!!!!!!!!!!'

    #pub.publish(test)

def onOff_cb(onOff):
    global controller_onOff
    global new_compass
    controller_onOff=onOff.data
    new_compass=True
    #print 'new controller onOff'


if __name__ == '__main__':
    rospy.init_node('heading_control') 
    rospy.Subscriber('heading_demand', Float32, heading_demand_cb)
    rospy.Subscriber('sway_demand', Float32, sway_demand_cb)
    rospy.Subscriber('compass_out', compass_out, compass_cb)
    rospy.Subscriber('Heading_onOFF', Bool, onOff_cb)
    rospy.loginfo("Heading controller (hover) online")
    low_level_control()
